#ifndef L_VERSION
#   define L_VERSION 1120104
#endif
#include <llib.h>

using namespace llib;
using namespace llib::geom;

#include <vector>
#include <iostream>
using namespace std;

class NormalCone2 {
public:
    Vec3f normal;
    Float angle;
};

Boolean MergeFilter(const NormalCone2& first, const NormalCone2& second, NormalCone2& res)
{
    // validate both normal cones
    if(first.angle > math::PI_2 || second.angle > math::PI_2) {
        // not valid merge
        res.angle = (Float)math::PI;
        res.normal[0] = 1.f;
        return TRUE;
    }

    // angle theta : the angle between two normal vector
    Float cosTheta = first.normal * second.normal;
    // angle delta : angle1 - angle2
    Float delta = first.angle - second.angle;
    Float cosDelta = cos(delta);

    // check enclosure
    if(cosTheta > cosDelta) {
        // enclosure here
        res = delta > 0 ? first : second;
        return TRUE;
    }

    return FALSE;
}

NormalCone2 Merge_real_0(const NormalCone2& first, const NormalCone2& second)
{
    Float normalAngle = acos(first.normal * second.normal);
    Float angle = (normalAngle + first.angle + second.angle) / 2;
    Float beta = angle - first.angle;
    Float cosBeta = cos(beta);
    const static Float Threshold = 0.999f;
    Vec3f firstBound = first.normal;
    Vec3f secondBound = second.normal;
    Vec3f direction;
    do {
        direction = firstBound + secondBound;
        direction.Normalize();
        if(direction * first.normal > cosBeta)
            firstBound = direction;
        else
            secondBound = direction;
    } while(firstBound * secondBound < Threshold);
    NormalCone2 res;
    res.angle = angle;
    res.normal = direction;
    return res;
}

NormalCone2 Merge0(const NormalCone2& first, const NormalCone2& second)
{
    NormalCone2 res;
    if(MergeFilter(first,second,res)) return res;
    return Merge_real_0(first,second);
}

//Int32 counter = 0;

NormalCone2 Merge_real_1(const NormalCone2& first, const NormalCone2& second)
{
    Float normalAngle = acos(first.normal * second.normal);
    Float angle = (normalAngle + first.angle + second.angle) / 2;

    // prepare the equation : P*x^2 + Q*x + R = 0
    Float A = first.normal * first.normal;
    Float B = first.normal * second.normal;
    Float C = second.normal * second.normal;
    Vec3f N2 = first.normal + second.normal;
    Float D = cos((first.angle - second.angle) / 2);
    D = (D * D) * (N2 * N2);
    Float E = B + C;
    Float F = A + B;
    Float P = E * E - C * D;
    Float Q = (E * F - B * D) * 2;
    Float R = F * F - A * D;
    vector<Float> root = math::EquationSolver::SolveQuadraticEquation(P,Q,R);

    // get w
    Float w;
    if(SZ(root) == 2) {
#ifdef _DEBUG
        Float t0 = root[0];
        Float t1 = root[1];
#endif
        w = (first.angle > second.angle) ? root[0] : root[1];
    } else if(SZ(root) == 1) {
        w = root[0];
        //cout << "Get single quation root. w = " << w << endl;
        //++counter;
    } else { // sz == 0
        w = -Q / P / 2;
        //cout << "Get single quation root. w = " << w << endl;
        //++counter;
    }

    Vec3f normal = first.normal + w * second.normal;
    normal.Normalize();
    NormalCone2 res;
    res.angle = angle;
    res.normal = normal;
    return res;
}

NormalCone2 Merge1(const NormalCone2& first, const NormalCone2& second)
{
    NormalCone2 res;
    if(MergeFilter(first,second,res)) return res;
    return Merge_real_1(first,second);
}

#include <boost/timer.hpp>
using namespace boost;

void Test_120109()
{
    NormalCone2 nc0,nc1;
    NormalCone2 res0,res1;
    const Int32 testCaseNum = 500000;
    Float checkVal0, checkVal1;
    Float Threshold = 0.995f;
    vector<NormalCone2> ncs0, ncs1;

    FOR(I,0,testCaseNum) {
        // randomize test data
        FOR(J,0,3) {
            nc0.normal[J] = llib::math::RndRange(-1.f,1.f);
            nc1.normal[J] = llib::math::RndRange(-1.f,1.f);
        }
        nc0.normal.Normalize();
        nc1.normal.Normalize();
        nc0.angle = llib::math::RndRange(0.f,Float(math::PI));
        nc1.angle = llib::math::RndRange(0.f,Float(math::PI));

        if(nc0.normal * nc1.normal < 0) {
            --I;
            continue;
        }

        if(MergeFilter(nc0,nc1,res0)) {
            --I;
            continue;;
        }

        ncs0.push_back(nc0);
        ncs1.push_back(nc1);
    }

    timer t;
    Int32 threExceed = 0;
    FOR(I,0,testCaseNum) {
        // the merge process
        res0 = Merge0(ncs0[I],ncs1[I]);
        res1 = Merge1(ncs0[I],ncs1[I]);
        checkVal0 = abs(res0.angle - res1.angle);
        checkVal1 = res0.normal * res1.normal;
        if(checkVal1 < Threshold) ++threExceed;
    }
    cout << "Threshold exceed: " << threExceed * 100.0 / testCaseNum << "%" << endl;

    //cout << "Counter = " << counter << endl;

    t.restart();
    FOR(I,0,testCaseNum) {
        res0 = Merge0(ncs0[I],ncs1[I]);
    }
    cout << "Merge0: " << t.elapsed() << endl;

    t.restart();
    FOR(I,0,testCaseNum) {
        res1 = Merge1(ncs0[I],ncs1[I]);
    }
    cout << "Merge1: " << t.elapsed() << endl;
}
